#include "main.h"
#include "math.h"
#include "tim.h"
#include "Motor_Control.h"
#include "adc.h"
#include "encoder.h"
#include "arm_math.h"
#include "pid.h"
#include "usart.h"
#include "SEGGER_RTT.h"
#include "stm32h7xx_it.h"
#include "74HC4051PW.h"

PID_TypeDef VD;
PID_TypeDef VQ;
PID_TypeDef Speed;
PID_TypeDef Position;
VOLTAGE_DQ_DEF Voltage_DQ;
CURRENT_DQ_DEF Current_DQ;
VOTAGE_ABC_DEF V_DATA;
VOLTAGE_ALPHA_BETA_DEF Voltage_Alpha_Beta;
Current_ABC_DEF I_DATA;
CURRENT_ALPHA_BETA_DEF I_ALPHA_BETA_DATA;
TRANSFER_COS_SIN_DEF TRANSFER_COS_SIN;

float Target_Position_Pre = 0;
float Target_Position = 0;
float Temperature = 0;
float Circle_Num = 0;
int OFFSET_A = 32768;
int OFFSET_B = 32768;
uint16_t OFFSET_C = 32768;
int Inject_ADC_Value[4];
uint16_t ADC_OFFSET_A[10];
uint16_t ADC_OFFSET_B[10];
uint16_t ADC_OFFSET_C[10];
uint8_t Start_Flag = False;
uint16_t Count_ADC;

float Average_Index;

Averge_Filter AF_1, AF_2, AF_3, AF_4;

void ADC_Injection_Init()
{
  HAL_ADCEx_InjectedStart_IT(&hadc1);
  HAL_ADCEx_InjectedStart_IT(&hadc3);
  HAL_ADCEx_Calibration_Start(&hadc1,ADC_CALIB_OFFSET,ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc3,ADC_CALIB_OFFSET,ADC_SINGLE_ENDED);
}

inline void AF_update(Averge_Filter * af, const float data)
{
  af->out += (data - af->buffer[af->idx]) / 5.f;
  af->buffer[af->idx] = data;
  af->idx = af->idx == 4 ? 0 : (af->idx + 1);
}

inline void AF_init(Averge_Filter * af) {
  memset(af->buffer, 0, sizeof(af->buffer));
  af->out = 0.f;
  af->idx = 0;
}

void PWM_Init()
{
  //PMW Start
  HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_4);

  HAL_TIMEx_PWMN_Start(&htim8,TIM_CHANNEL_1);
  HAL_TIMEx_PWMN_Start(&htim8,TIM_CHANNEL_2);
  HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);

  //开启pwm通道4用于触发ADC注入采样,关闭上管看下管采样正不正确
  __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, 0);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
  __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4, PWM_PERIOD-5);
  HAL_Delay(500);
}


void PID_Init()
{
  pid_init(&VD);
  pid_init(&VQ);
  pid_init(&Speed);
  pid_init(&Position);
  VD.f_param_init(&VD,PID_CURRENT,20,16384,0,0,16384,0,0.6f,0.2f,0.0f);//0.5.0.2
  VQ.f_param_init(&VQ,PID_CURRENT,20,16384,0,0,16384,0,0.6f,0.2f,0.0f);
  Speed.f_param_init(&Speed,PID_Speed,80,80,0,0,80,0,2.7f,0.008f,0.0f);//6.0 0.001
  Position.f_param_init(&Position,PID_Position,50,50,0,0,16384,0,0.005f,0.0f,0.0f);//
  Target_Position = Position.target = Encoder_Data_Real.Now;
  Speed.target = 0.0f;
  VD.target = 0.0f;
  VQ.target = 0.0f;
}

//算出来传进来的角度的cos和sin值
inline void Angle_To_Cos_Sin(float angle_temp,TRANSFER_COS_SIN_DEF * cos_sin_temp)
{
  cos_sin_temp->cos = arm_cos_f32(angle_temp);
  cos_sin_temp->sin = arm_sin_f32(angle_temp);
}

//进行Clarke变换
inline void Clarke_Transfer(Current_ABC_DEF Current_ABC, CURRENT_ALPHA_BETA_DEF * Current_alpha_beta_temp)
{
  Current_alpha_beta_temp->I_alpha = (Current_ABC.Ia - (Current_ABC.Ib + Current_ABC.Ic) * 0.5F) * 2.0F / 3.0F;
  Current_alpha_beta_temp->I_beta = (Current_ABC.Ib - Current_ABC.Ic) * 0.866025388F * 2.0F / 3.0F;
}
//进行Park变换

inline void Park_Transfer(CURRENT_ALPHA_BETA_DEF Current_Alpha_Beta_Temp, TRANSFER_COS_SIN_DEF Cos_Sin_Temp, CURRENT_DQ_DEF *Current_Dq_Temp)
{
  Current_Dq_Temp->Id = (Current_Alpha_Beta_Temp.I_alpha * Cos_Sin_Temp.cos + Current_Alpha_Beta_Temp.I_beta * Cos_Sin_Temp.sin);
  Current_Dq_Temp->Iq = (-Current_Alpha_Beta_Temp.I_alpha * Cos_Sin_Temp.sin + Current_Alpha_Beta_Temp.I_beta * Cos_Sin_Temp.cos);
}

inline void Rev_Park_Transfer(VOLTAGE_DQ_DEF V_Dq_Temp,TRANSFER_COS_SIN_DEF Cos_Sin_Temp,VOLTAGE_ALPHA_BETA_DEF * V_Alpha_Beta_Temp)
{
  V_Alpha_Beta_Temp->Valpha = Cos_Sin_Temp.cos * V_Dq_Temp.Vd - Cos_Sin_Temp.sin * V_Dq_Temp.Vq;
  V_Alpha_Beta_Temp->Vbeta = Cos_Sin_Temp.sin * V_Dq_Temp.Vd + Cos_Sin_Temp.cos * V_Dq_Temp.Vq;
}

void Rev_Clark_Transfer(VOLTAGE_ALPHA_BETA_DEF V_AB_TEMP,VOTAGE_ABC_DEF * V_ABC_TEMP)
{
  V_ABC_TEMP->Va = V_AB_TEMP.Valpha;
  V_ABC_TEMP->Vb = (1.73205078F*V_AB_TEMP.Vbeta - V_AB_TEMP.Valpha)/2;
  V_ABC_TEMP->Vc = -(V_AB_TEMP.Valpha + 1.73205078F*V_AB_TEMP.Vbeta)/2;
}

void Move_Forward(float Array[])
{
  for(int i = 0;i < 4;i++)
  {
    Array[i] = Array[i+1];
  }
}

float Get_Average(float Array[])
{
  float max = 0;
  for (int i = 0; i < 5; i++)
  {
    max = max + Array[i];
  }
  max = max / 5;
  return max;
}

// void Get_Current()
// {
//   Inject_ADC_Value[0] = ADC1->JDR1;
//   Inject_ADC_Value[1] = ADC1->JDR2;
//   V_DATA.Vb = (-(float )OFFSET_B + (float )Inject_ADC_Value[0])/2048.0f*3.3f;
//   V_DATA.Vc = (-(float )OFFSET_C + (float )Inject_ADC_Value[1])/2048.0f*3.3f;
//   I_DATA.Ic = V_DATA.Vc/40/0.015f;
//   I_DATA.Ib = V_DATA.Vb/40/0.015f;
// //  I_DATA.Ic = V_DATA.Vc/40;
// //  I_DATA.Ib = V_DATA.Vb/40;
//   I_DATA.Ia = -I_DATA.Ic-I_DATA.Ib;
//   AF_update(&AF_1, I_DATA.Ia);
//   AF_update(&AF_2, I_DATA.Ib);
//   AF_update(&AF_3, I_DATA.Ic);
//   I_DATA.Ia = AF_1.out;
//   I_DATA.Ib = AF_2.out;
//   I_DATA.Ic = AF_3.out;
// }

void Get_Current_ABC()
{
  //Get the value of the Voltage
  Inject_ADC_Value[0] = HAL_ADCEx_InjectedGetValue(&hadc3,ADC_INJECTED_RANK_1);
  Inject_ADC_Value[1] = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1);
  Inject_ADC_Value[2] = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2);//hall sensor

  //Change the Value of the ADC into the Real Voltage Value
  V_DATA.Va = (float)(-OFFSET_A + Inject_ADC_Value[0])/32768.0f*82.5f;
  V_DATA.Vb = (float)(-OFFSET_B + Inject_ADC_Value[1])/32768.0f*82.5f;

  //Get the real Value of the Current
  I_DATA.Ia = V_DATA.Va;
  I_DATA.Ib = V_DATA.Vb;
  I_DATA.Ic = 0 - I_DATA.Ia - I_DATA.Ib;

}

void OFFSET_INIT()
{
  Inject_ADC_Value[0] = HAL_ADCEx_InjectedGetValue(&hadc3,ADC_INJECTED_RANK_1);
  Inject_ADC_Value[1] = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1);
  ADC_OFFSET_A[Count_ADC] = Inject_ADC_Value[0];
  ADC_OFFSET_B[Count_ADC] = Inject_ADC_Value[1];
  Count_ADC++;
  if (Count_ADC == 10)
  {
    int SUM_A = 0;
    int SUM_B = 0;
    for (int i = 0; i < 10; i++)
    {
      SUM_A += ADC_OFFSET_A[i];
      SUM_B += ADC_OFFSET_B[i];
    }
    OFFSET_A = SUM_A / 10;
    OFFSET_B = SUM_B / 10;
    Count_ADC = 0;
    Start_Flag = True;
  }
}

void Electrical_Angle_Init()
{
  Voltage_DQ.Vd = 0.7f;
  Voltage_DQ.Vq = 0;
  Angle_To_Cos_Sin(0,&TRANSFER_COS_SIN);
  Rev_Park_Transfer(Voltage_DQ,TRANSFER_COS_SIN,&Voltage_Alpha_Beta);
  SVPWM_Control(Voltage_Alpha_Beta,48,PWM_TIM_PULSE_TPWM);
  // HAL_UART_Transmit(&huart5,&tx_position,1,0xffff);
  // HAL_Delay(2000);
  // Encoder_Data_Real = (float)TR_Encoder_Position_Data;
  // Electrical_Angle_Offset = Get_Electrical_Angle();
  // __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, PWM_PERIOD);
  // __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, PWM_PERIOD);
  // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, PWM_PERIOD);
//  cprintf(&huart2,"the offset is %f\n",Electrical_Angle_Offset);
}

float Get_Temperature()
{
  HAL_GPIO_WritePin(ADC_SW0_GPIO_Port, ADC_SW0_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(ADC_SW1_GPIO_Port, ADC_SW1_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(ADC_SW2_GPIO_Port, ADC_SW2_Pin, GPIO_PIN_SET);
  float temp_value = (float)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_4) / 65535.0f * 3.3f;
  float Real_Resistance = 4700 * temp_value / (3.3f - temp_value);
  Temperature = 1/(logf(Real_Resistance/10000)/3455+1/(298.15)) - 273.15;
  return Temperature;
}

void Temperature_Control()
{
  if(Temperature >= 80)
  {
    HAL_TIM_PWM_Stop(&htim8,TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(&htim8,TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_3);
  }
}

void SVPWM_Control(VOLTAGE_ALPHA_BETA_DEF V_ALPHA_BETA_TEMP,float UDC_TEMP,float TPWM_TEMP)
{
  static float Ta, Tb, Tc;
  static float Tcmp1, Tcmp2, Tcmp3;
  static float Tx, Ty;
  static float F_Temp;
  static uint8_t A, B, C;
  static uint8_t N;
  uint8_t sector = 0;

  Tcmp1 = 0.0f;
  Tcmp2 = 0.0f;
  Tcmp3 = 0.0f;
  if (V_ALPHA_BETA_TEMP.Vbeta > 0.0f)
  {
    A = 1;
  }
  else
  {
    A = 0;
  }

  if ((1.73205078f * V_ALPHA_BETA_TEMP.Valpha - V_ALPHA_BETA_TEMP.Vbeta) / 2.0f > 0.0f)
  {
    B = 1;
  }
  else
  {
    B = 0;
  }

  if ((-1.73205078f * V_ALPHA_BETA_TEMP.Valpha - V_ALPHA_BETA_TEMP.Vbeta) / 2.0f > 0.0f)
  {
    C = 1;
  }
  else
  {
    C = 0;
  }
  N = A + 2 * B + 4 * C;

  switch (N)
  {
    case 1:
      sector = 2;
      break;
    case 2:
      sector = 6;
      break;
    case 3:
      sector = 1;
      break;
    case 4:
      sector = 4;
      break;
    case 5:
      sector = 3;
      break;
    case 6:
      sector = 5;
      break;
    default: ;
  }

//  if (sector != sector_pre)
//  {
//    sector_pre = sector;
//    cprintf(&huart2,"the sector is %d\n",sector);
//  }

  //根据扇区选择当前要用的合成矢量
  switch (sector)
  {
    case 1:
      Tx = (1.5F * V_ALPHA_BETA_TEMP.Valpha - 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      Ty = 1.73205078F * V_ALPHA_BETA_TEMP.Vbeta * (TPWM_TEMP / UDC_TEMP);
      break;

    case 2:
      Tx = (-1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      Ty = (1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      break;

    case 3:
      Tx = 1.73205078F * V_ALPHA_BETA_TEMP.Vbeta * (TPWM_TEMP / UDC_TEMP);
      Ty = -(1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      break;

    case 4:
      Tx = -1.73205078F * V_ALPHA_BETA_TEMP.Vbeta * (TPWM_TEMP / UDC_TEMP);
      Ty = (-1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      break;

    case 5:
      Tx = -(1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      Ty = (1.5F * V_ALPHA_BETA_TEMP.Valpha - 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      break;

    case 6:
      Tx = (1.5F * V_ALPHA_BETA_TEMP.Valpha + 0.866025388F * V_ALPHA_BETA_TEMP.Vbeta) * (TPWM_TEMP / UDC_TEMP);
      Ty = -1.73205078F * V_ALPHA_BETA_TEMP.Vbeta * (TPWM_TEMP / UDC_TEMP);
      break;
    default: ;
  }


  F_Temp = Tx + Ty;
//  cprintf(&huart2,"Tx is %f,Ty is %f,TPWM_PWM is %f,F_Temp is %f\n",Tx,Ty,TPWM_TEMP,F_Temp);
  //过调制处理
  if (F_Temp > TPWM_TEMP * 0.95)
  {
    Tx = Tx / F_Temp * TPWM_TEMP * 0.95f;
    Ty = Ty / F_Temp * TPWM_TEMP * 0.95f;
//    Tx = Tx / F_Temp * TPWM_TEMP ;
//    Ty = Ty / F_Temp * TPWM_TEMP ;
  }
//  cprintf(&huart2,"Tx is %f,Ty is %f,TPWM_PWM is %f,F_Temp is %f\n",Tx,Ty,TPWM_TEMP,F_Temp);

  Ta = (TPWM_TEMP - Tx - Ty) / 4.0F;
  Tb = Tx / 2.0F + Ta ;
  Tc = Ty / 2.0F + Tb ;

//  cprintf(&huart2,"ta=%f,tb=%f,tc=%f\n",Ta,Tb,Tc);

  switch (sector)
  {
    case 1:
      Tcmp1 = Ta;
      Tcmp2 = Tb;
      Tcmp3 = Tc;
      break;

    case 2:
      Tcmp1 = Tb;
      Tcmp2 = Ta;
      Tcmp3 = Tc;
      break;

    case 3:
      Tcmp1 = Tc;
      Tcmp2 = Ta;
      Tcmp3 = Tb;
      break;

    case 4:
      Tcmp1 = Tc;
      Tcmp2 = Tb;
      Tcmp3 = Ta;

      break;

    case 5:
      Tcmp1 = Tb;
      Tcmp2 = Tc;
      Tcmp3 = Ta;
      break;

    case 6:
      Tcmp1 = Ta;
      Tcmp2 = Tc;
      Tcmp3 = Tb;
      break;
    default: ;
  }

  // cprintf(&huart5,"%f,%f,%f\n",Tcmp1,Tcmp2,Tcmp3);//321
  __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, (uint16_t)Tcmp1);
  __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, (uint16_t)Tcmp2);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint16_t)Tcmp3);
}